// DetectCls.cpp: implementation of the CDetectModelCls class.
//
//////////////////////////////////////////////////////////////////////


#include "DetectCls.h"
#include <stdio.h>
#include <string>
#include <math.h>
#include <stdlib.h>
#include "ModelDetectData.h"
#include <android/log.h>
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////


//#define FLOAT_INT_MOVE 10

//
#define LOG_TAG "UHYLab"
#define LOGE(...) ((void)__android_log_print(ANDROID_LOG_ERROR, LOG_TAG, __VA_ARGS__))

#ifndef max
#define max(a, b)            (((a) > (b)) ? (a) : (b))
#endif

#ifndef min
#define min(a, b)            (((a) < (b)) ? (a) : (b))
#endif

const int rect_wd = 17;
const int rect_ht = 17;
#define float_int_rate 1024

const int nDetectLevel_OriLAB = 7; // 7;

const float total_delta_rate_one_level = 1.25992;
const int level_num_one_region = 3;

const int LAB_block_size_num = 1;
const int LAB_buf_num = LAB_block_size_num * 3;
static int LAB_block_size[LAB_block_size_num][3] = {2, 2, 2};//,    1,2,0};

CDetectModelCls::CDetectModelCls() {
}

CDetectModelCls::CDetectModelCls(int nDetectLevel) {
    int i, j, n;
    m_nDetectLevel = nDetectLevel;
    pFastDetectModel = (FAST_DETECT_MODEL_FIRST *) malloc(
            sizeof(FAST_DETECT_MODEL_FIRST) * nDetectLevel);
    for (i = 0; i < nDetectLevel; ++i) {
        pFastDetectModel[i].n_step = 0;//???????
        pFastDetectModel[i].alpha_thres = 0;//Adaboost????????alpha????��?????
        pFastDetectModel[i].pModel = NULL;
    }
    FD16_pFastDetectModelTemp = (FD16_FAST_DETECT_MODEL_FIRST_CHAR *) malloc(
            sizeof(FD16_FAST_DETECT_MODEL_FIRST_CHAR) * nDetectLevel);
    for (i = 0; i < nDetectLevel; ++i) {
        FD16_pFastDetectModelTemp[i].n_step = 0;
        FD16_pFastDetectModelTemp[i].alpha_thres = 0;
        FD16_pFastDetectModelTemp[i].pModel = NULL;
    }

    nrate = 24;
    pSampleRate = (PYRAMID_SAMPLE_STRUCT *) malloc(sizeof(PYRAMID_SAMPLE_STRUCT) * nrate);
    n = 0;
    for (i = 0; i < nrate; i += level_num_one_region) {
        for (j = 0; j < level_num_one_region; ++j) {
            pSampleRate[i + j].deflate_rate = 1.0 * pow(total_delta_rate_one_level, j);
            pSampleRate[i + j].down_sample_move_2 = n;
            pSampleRate[i + j].down_sample_rate_2 = int(pow(2.0, n) + 0.5);
            pSampleRate[i + j].total_deflate_rate =
                    pSampleRate[i + j].down_sample_rate_2 * pSampleRate[i + j].deflate_rate;
        }
        n++;
    }

    m_detect_end_search_scale_num = 4;
    m_detect_start_search_scale_num = 0;

    m_Candidate_Combine_Weight_MinThres = 1;
    m_Candidate_Combine_Conf_MinThres = -128;
    m_Detect_Search_XShift = 1;
    m_Detect_Search_YShift = 1;

}

CDetectModelCls::~CDetectModelCls() {
    int i;

    for (i = 0; i < m_nDetectLevel; ++i) {
        if (pFastDetectModel[i].pModel)
            free(pFastDetectModel[i].pModel);
    }
    if (pFastDetectModel)
        free(pFastDetectModel);
    if (pSampleRate)
        free(pSampleRate);


    for (i = 0; i < m_nDetectLevel; ++i) {
        if (FD16_pFastDetectModelTemp[i].pModel)
            free(FD16_pFastDetectModelTemp[i].pModel);
    }
    if (FD16_pFastDetectModelTemp)
        free(FD16_pFastDetectModelTemp);
}

//??????
bool CDetectModelCls::LoadDetectModel(const char *filename) {
    FILE *file;
    int i, j, k;

    file = fopen(filename, "rb");
    if (file == NULL)
        return false;

    for (i = 0; i < m_nDetectLevel; ++i) {
        fread(&(pFastDetectModel[i].n_step), sizeof(int), 1, file);
        fread(&(pFastDetectModel[i].alpha_thres), sizeof(int), 1, file);
        if (pFastDetectModel[i].pModel)
            free(pFastDetectModel[i].pModel);
        pFastDetectModel[i].pModel = (FAST_ADABOOST_DETECT_PARAM_FIRST *) malloc(
                sizeof(FAST_ADABOOST_DETECT_PARAM_FIRST) * pFastDetectModel[i].n_step);
        fread(pFastDetectModel[i].pModel, sizeof(FAST_ADABOOST_DETECT_PARAM_FIRST),
              pFastDetectModel[i].n_step, file);
    }
/*	int max_data = 0, min_data = 0;
	for(i = 0;i < nDetectLevel; ++i)
		for(j = 0;j < pFastDetectModel[i].n_step; ++j)
		{
			for(k = 0;k < 256; ++k)
			{
				if(pFastDetectModel[i].pModel[j].lab_histo[k] > max_data)
					max_data = pFastDetectModel[i].pModel[j].lab_histo[k];
				if(pFastDetectModel[i].pModel[j].lab_histo[k] < min_data)
					min_data = pFastDetectModel[i].pModel[j].lab_histo[k];
			}

		}*/
    fclose(file);


    return true;
}

//??????
bool CDetectModelCls::LoadDetectModel() {

    int i, j, k;
    unsigned char *model_ptr_temp = model_data_buffer;
    for (i = 0; i < m_nDetectLevel; ++i) {
        pFastDetectModel[i].n_step = *((int *) model_ptr_temp);
        model_ptr_temp += sizeof(int);

        pFastDetectModel[i].alpha_thres = *((float *) model_ptr_temp);
        model_ptr_temp += sizeof(float);

        if (pFastDetectModel[i].pModel) {
            free(pFastDetectModel[i].pModel);
            pFastDetectModel[i].pModel = NULL;
        }
        pFastDetectModel[i].pModel = (FAST_ADABOOST_DETECT_PARAM_FIRST *) malloc(
                sizeof(FAST_ADABOOST_DETECT_PARAM_FIRST) * pFastDetectModel[i].n_step);

        memcpy(pFastDetectModel[i].pModel, model_ptr_temp,
               sizeof(FAST_ADABOOST_DETECT_PARAM_FIRST) * pFastDetectModel[i].n_step);
        model_ptr_temp += sizeof(FAST_ADABOOST_DETECT_PARAM_FIRST) * pFastDetectModel[i].n_step;
    }

    return true;
}


void CDetectModelCls::SetDetectParam(DETECT_PARAM *pDetectParam) {

    if (!pDetectParam)
        return;

    m_detect_end_search_scale_num = pDetectParam->detect_end_search_scale_num;
    m_detect_start_search_scale_num = pDetectParam->detect_start_search_scale_num;

    m_Candidate_Combine_Weight_MinThres = pDetectParam->Candidate_Combine_Weight_MinThres;
    m_Candidate_Combine_Conf_MinThres = pDetectParam->Candidate_Combine_Conf_MinThres;
    m_Detect_Search_XShift = pDetectParam->Detect_Search_XShift;
    m_Detect_Search_YShift = pDetectParam->Detect_Search_YShift;
}

//?????????????????????????��???????????????????
bool CDetectModelCls::JudgeCandidateRectImage_Single(FAST_DETECT_MODEL_FIRST *pFeatModel, int wd,
                                                     int ht,
                                                     int **block_sum, int x, int y, float *conf) {
    bool bSim = false;
    float alpha_res;
    int feat, feat_num;
    int i;
    FAST_ADABOOST_DETECT_PARAM_FIRST *pCurrentModel;

    pCurrentModel = pFeatModel->pModel;

    alpha_res = 0;
    for (i = 0; i < pFeatModel->n_step; ++i) {
        feat = FastCalOneLabFeat_Single(pCurrentModel + i, wd, ht, x, y, block_sum);
        alpha_res += pCurrentModel[i].lab_histo[feat];
    }
    *conf = alpha_res;
    if (alpha_res >= pFeatModel->alpha_thres)
        bSim = true;

    return bSim;
}


//?????????????????????????��???????????????????
bool CDetectModelCls::JudgeCandidateRectImage_LAB(FAST_DETECT_MODEL_FIRST *pFeatModel, int wd,
                                                  int ht,
                                                  int **lab_sum, int x, int y, float *conf,
                                                  float prev_conf) {
    bool bSim = false;
    float alpha_res;
    int feat, feat_num;
    int i;
    FAST_ADABOOST_DETECT_PARAM_FIRST *pCurrentModel;
    int block_top, block_left;

    pCurrentModel = pFeatModel->pModel;

    alpha_res = prev_conf;
    for (i = 0; i < pFeatModel->n_step; ++i) {
        block_top = y + pCurrentModel[i].top;
        block_left = x + pCurrentModel[i].left;
        feat = lab_sum[ht * pCurrentModel[i].feat_flag + block_top][block_left];
        alpha_res += pCurrentModel[i].lab_histo[feat];
    }
    *conf = alpha_res;
    if (alpha_res >= pFeatModel->alpha_thres)
        bSim = true;

    return bSim;
}

//?????????????????????????��???????????????????
bool CDetectModelCls::JudgeCandidateRectImage(int **lab_data, int cur_level,
                                              int wd, int ht, int x, int y, float &res_conf) {
    int i;
    float conf = 0, prev_conf = 0;
    res_conf = 0;
    for (i = 0; i < cur_level; ++i) {

        if (!JudgeCandidateRectImage_LAB(pFastDetectModel + i, wd, ht, lab_data, x, y, &conf,
                                         prev_conf))
            break;
        prev_conf = conf;
        res_conf = conf;

    }
    if (i < cur_level)
        return false;

    return true;
}

int CDetectModelCls::FastCalOneLabFeat_Single(FAST_ADABOOST_DETECT_PARAM_FIRST *pModel,
                                              int wd, int ht, int left, int top,
                                              int **block_sum_ptr) {
    int i;
    int feat = 0;
    int block_sum, neigh_block_sum;
    int block_top, block_left, block_bottom, block_right;
    block_top = top + pModel->top;
    block_left = left + pModel->left;

    block_sum = block_sum_ptr[ht * pModel->feat_flag + block_top][block_left];

    for (i = 0; i < LAB_BLOCK_FEAT_NUM; ++i) {
        block_top = top + pModel->neigh_top[i];
        block_left = left + pModel->neigh_left[i];
        neigh_block_sum = block_sum_ptr[ht * pModel->neigh_feat_flag[i] + block_top][block_left];

        if (block_sum < neigh_block_sum)
            feat |= (1 << i);
    }
    return feat;
}


unsigned char IsCoverObject(DECTRECT tempObject1, DECTRECT tempObject2) {
    int coverLeft = max(tempObject1.left, tempObject2.left);
    int coverRight = min(tempObject1.right, tempObject2.right);
    int coverTop = max(tempObject1.top, tempObject2.top);
    int coverBottom = min(tempObject1.bottom, tempObject2.bottom);

    int area1;
    int area2;
    int minArea;
    int coverArea;
    int obj1_wd, obj2_wd;

    if (coverLeft < coverRight && coverTop < coverBottom) // ????
    {
        obj1_wd = tempObject1.right - tempObject1.left;
        obj2_wd = tempObject2.right - tempObject2.left;
        area1 = (tempObject1.right - tempObject1.left) * (tempObject1.bottom - tempObject1.top);
        area2 = (tempObject2.right - tempObject2.left) * (tempObject2.bottom - tempObject2.top);
        minArea = min(area1, area2);
        coverArea = (coverRight - coverLeft) * (coverBottom - coverTop);

        if ((obj1_wd >= (obj2_wd >> 1)) && (obj2_wd >= (obj1_wd >> 1)) &&
            (coverArea >= (minArea >> 1))) {
            return (1);//true );
        }
        else {
            return (0);//false );
        }
    }
    else {
        return (0);//false );
    }
}


int CDetectModelCls::CombineDetectedObjs(DETECT_DATA *pSquarePos, int nFace) {
    int i, j;
    signed char *bProFlag;
    int sum_num, nNewFace, n_neigh_faces;

    bProFlag = (signed char *) malloc(sizeof(signed char) * nFace);
    memset(bProFlag, 0, sizeof(signed char) * nFace);

    //??????????��????????????
    do {
        n_neigh_faces = 0;
        for (i = 0; i < nFace; ++i) {
            if (bProFlag[i])
                continue;
            for (j = 0; j < nFace; ++j) {
                if (j == i)
                    continue;
                if (bProFlag[j])
                    continue;
                if (IsCoverObject(pSquarePos[i].square_rect, pSquarePos[j].square_rect)) {
                    //????????????????????
                    sum_num = pSquarePos[i].weight + pSquarePos[j].weight;
                    pSquarePos[i].square_rect.left =
                            (pSquarePos[i].square_rect.left * pSquarePos[i].weight +
                             pSquarePos[j].square_rect.left * pSquarePos[j].weight) / sum_num;
                    pSquarePos[i].square_rect.top =
                            (pSquarePos[i].square_rect.top * pSquarePos[i].weight +
                             pSquarePos[j].square_rect.top * pSquarePos[j].weight) / sum_num;
                    pSquarePos[i].square_rect.right =
                            (pSquarePos[i].square_rect.right * pSquarePos[i].weight +
                             pSquarePos[j].square_rect.right * pSquarePos[j].weight) / sum_num;
                    pSquarePos[i].square_rect.bottom =
                            (pSquarePos[i].square_rect.bottom * pSquarePos[i].weight +
                             pSquarePos[j].square_rect.bottom * pSquarePos[j].weight) / sum_num;
                    pSquarePos[i].conf += pSquarePos[j].conf;
                    pSquarePos[i].weight += pSquarePos[j].weight;
                    bProFlag[j] = 1;
                    n_neigh_faces++;
                }
            }
        }
    } while (n_neigh_faces);

    nNewFace = 0;
    for (i = 0; i < nFace; ++i) {
        if (!bProFlag[i]) {
            if (pSquarePos[i].conf >= m_Candidate_Combine_Conf_MinThres &&
                pSquarePos[i].weight >= m_Candidate_Combine_Weight_MinThres) {
                pSquarePos[nNewFace] = pSquarePos[i];
                nNewFace++;
            }
        }
    }

    free(bProFlag);
    return nNewFace;
}


int CDetectModelCls::FastDetectAllSquarePosition_All(unsigned char **image, int ht, int wd,
                                                     DETECT_DATA *pSquarePos) {
    int nface = 0;
    int dwd;
    int i, j, k, l, m, n;
    int x, y;
    DEFLATE_IMAGE *dimage_data;
    DETECT_DATA cur_rect_data;
    int sigma;
    int norm_rate;
    int **norm_image, **sum_image;
    int **block_sum_data, **total_lab_data;
    int *rate_xy;
    int multi_rect_ht_wd = rect_ht * rect_wd;
    int feat_num;
    float sum_conf;

    int block_feat[5000];
    int multi_sigma = 1024;
    int std_norm_sigma = ((32 * float_int_rate) << 10);

    rate_xy = (int *) malloc(sizeof(int) * max(ht, wd));
    dimage_data = (DEFLATE_IMAGE *) malloc(sizeof(DEFLATE_IMAGE) * nrate);
    block_sum_data = f2i(ht * LAB_buf_num, wd);
    total_lab_data = f2i(ht * LAB_buf_num, wd);

    for (i = 0; i < nrate; ++i) {
        dimage_data[i].cur_rate = pSampleRate[i].total_deflate_rate;
        dimage_data[i].ori_ht = ht;
        dimage_data[i].ori_wd = wd;
        dimage_data[i].cur_ht = ht / dimage_data[i].cur_rate;
        dimage_data[i].cur_wd = wd / dimage_data[i].cur_rate;
        dimage_data[i].cur_image = f2b(dimage_data[i].cur_ht, dimage_data[i].cur_wd);
        dimage_data[i].sum_image = f2i(dimage_data[i].cur_ht, dimage_data[i].cur_wd);
    }


    //????????????��?????????????��???
    for (i = m_detect_start_search_scale_num; i < m_detect_end_search_scale_num; ++i) {
        if (i == 0) {
            for (j = 0; j < dimage_data[i].cur_ht; ++j)
                for (k = 0; k < dimage_data[i].cur_wd; ++k)
                    dimage_data[i].cur_image[j][k] = image[j][k];
        }
        else
            BilinearResizeImage_Down2(image, dimage_data[i].cur_image, wd, ht,
                                      dimage_data[i].cur_wd, dimage_data[i].cur_ht,
                                      pSampleRate[i].down_sample_rate_2,
                                      pSampleRate[i].down_sample_move_2);
        GetSumImages(dimage_data[i].cur_image, dimage_data[i].sum_image, dimage_data[i].cur_wd,
                     dimage_data[i].cur_ht);
        GetBlockSumHaarData_All(block_sum_data, dimage_data[i].sum_image, dimage_data[i].cur_wd,
                                dimage_data[i].cur_ht);
        GetBlockLABData_all(block_sum_data, total_lab_data, dimage_data[i].cur_wd,
                            dimage_data[i].cur_ht);

        for (y = 1; y < dimage_data[i].cur_ht; y += m_Detect_Search_YShift)
            for (x = 1; x < dimage_data[i].cur_wd; x += m_Detect_Search_XShift) {
                if (y + rect_ht >= dimage_data[i].cur_ht ||
                    x + rect_wd >= dimage_data[i].cur_wd)
                    continue;
                //???????????????????????????
                cur_rect_data.square_rect.left = x;
                cur_rect_data.square_rect.top = y;
                cur_rect_data.square_rect.right = x + rect_wd;
                cur_rect_data.square_rect.bottom = y + rect_ht;
                //??????????????????
                float conf = 0;
                if (!JudgeCandidateRectImage(total_lab_data,
                                             m_nDetectLevel, dimage_data[i].cur_wd,
                                             dimage_data[i].cur_ht,
                                             x, y, conf))
                    continue;

                pSquarePos[nface].square_rect.left = int(
                        cur_rect_data.square_rect.left * pSampleRate[i].total_deflate_rate + 0.5);
                pSquarePos[nface].square_rect.top = int(
                        cur_rect_data.square_rect.top * pSampleRate[i].total_deflate_rate + 0.5);
                pSquarePos[nface].square_rect.bottom = int(
                        cur_rect_data.square_rect.bottom * pSampleRate[i].total_deflate_rate + 0.5);
                pSquarePos[nface].square_rect.right = int(
                        cur_rect_data.square_rect.right * pSampleRate[i].total_deflate_rate + 0.5);
                pSquarePos[nface].scale = i;
                pSquarePos[nface].weight = 1;
                pSquarePos[nface].conf = conf;

                nface++;
            }
    }

    for (i = 0; i < nrate; ++i) {
        FreeArray_BYTE(dimage_data[i].cur_image, dimage_data[i].cur_ht, dimage_data[i].cur_wd);
        FreeArray_int(dimage_data[i].sum_image, dimage_data[i].cur_ht, dimage_data[i].cur_wd);
    }

    char str[256];

    free(dimage_data);
    free(rate_xy);
    FreeArray_int(block_sum_data, ht * LAB_buf_num, wd);
    FreeArray_int(total_lab_data, ht * LAB_buf_num, wd);
    return nface;
}


void CDetectModelCls::GetBlockSumHaarData_All(int **block_sum_data, int **sum_image, int wd,
                                              int ht) {
    int ii, i, j, k, l;
    int **haar_data_x, **haar_data_y;
    int **haar_sum_x, **haar_sum_y;
    int **sum_data, **sum_data_haar_x, **sum_data_haar_y;
    int haar_wd, haar_ht, haar_move;

    haar_data_x = f2i(ht, wd);
    haar_data_y = f2i(ht, wd);
    haar_sum_x = f2i(ht, wd);
    haar_sum_y = f2i(ht, wd);

    for (i = 0; i < ht; ++i) {
        memset(haar_data_x[i], 0, sizeof(int) * wd);
        memset(haar_data_y[i], 0, sizeof(int) * wd);
        memset(haar_sum_x[i], 0, sizeof(int) * wd);
        memset(haar_sum_y[i], 0, sizeof(int) * wd);
    }
    for (ii = 0; ii < LAB_block_size_num; ++ii) {
        haar_wd = LAB_block_size[ii][0];
        haar_ht = LAB_block_size[ii][1];
        haar_move = LAB_block_size[ii][2];

        sum_data = block_sum_data + ii * 3 * ht;
        sum_data_haar_x = block_sum_data + ii * 3 * ht + ht;
        sum_data_haar_y = block_sum_data + ii * 3 * ht + 2 * ht;

        for (i = 1; i < ht + 1 - haar_ht; i++)
            for (j = 1; j < wd + 1 - haar_wd; j++) {
                sum_data[i][j] = GetRectSumImage_Fast(sum_image, i - 1, j - 1, i + haar_ht - 1,
                                                      j + haar_wd - 1);
                sum_data[i][j] = (sum_data[i][j] >> haar_move);
            }
        for (i = haar_ht; i < ht + 1 - haar_ht; ++i)
            for (j = haar_wd; j < wd + 1 - haar_wd; ++j) {
                haar_data_x[i][j] = abs(sum_data[i][j] + sum_data[i - haar_ht][j]
                                        - sum_data[i][j - haar_wd] -
                                        sum_data[i - haar_ht][j - haar_wd]);
                haar_data_y[i][j] = abs(sum_data[i][j] + sum_data[i][j - haar_wd]
                                        - sum_data[i - haar_ht][j] -
                                        sum_data[i - haar_ht][j - haar_wd]);
            }

        GetSumImages(haar_data_x, haar_sum_x, wd, ht);
        GetSumImages(haar_data_y, haar_sum_y, wd, ht);
        for (k = 1; k < ht + 1 - haar_ht; ++k)
            for (l = 1; l < wd + 1 - haar_wd; ++l) {
                sum_data_haar_x[k][l] = GetRectSumImage_Fast(haar_sum_x, k - 1, l - 1,
                                                             k + haar_ht - 1, l + haar_wd - 1);
                sum_data_haar_y[k][l] = GetRectSumImage_Fast(haar_sum_y, k - 1, l - 1,
                                                             k + haar_ht - 1, l + haar_wd - 1);
            }
    }
    FreeArray_int(haar_data_x, ht, wd);
    FreeArray_int(haar_data_y, ht, wd);
    FreeArray_int(haar_sum_x, ht, wd);
    FreeArray_int(haar_sum_y, ht, wd);
}

void CDetectModelCls::GetBlockLABData_all(int **block_sum_data, int **total_lab_data, int wd,
                                          int ht) {
    int i, j, ii;
    int **lab_data, **lab_data_x, **lab_data_y;
    int **sum_data, **sum_data_haar_x, **sum_data_haar_y;
    int cen_block;
    int lab_feat;
    int haar_ht, haar_wd;

    for (ii = 0; ii < LAB_block_size_num; ++ii) {
        haar_wd = LAB_block_size[ii][0];
        haar_ht = LAB_block_size[ii][1];

        lab_data = total_lab_data + ii * 3 * ht;
        lab_data_x = total_lab_data + ii * 3 * ht + ht;
        lab_data_y = total_lab_data + ii * 3 * ht + 2 * ht;
        sum_data = block_sum_data + ii * 3 * ht;
        sum_data_haar_x = block_sum_data + ii * 3 * ht + ht;
        sum_data_haar_y = block_sum_data + ii * 3 * ht + 2 * ht;

        for (i = 0; i < ht + 1 - 3 * haar_ht; ++i)
            for (j = 0; j < wd + 1 - 3 * haar_wd; ++j) {
                lab_feat = 0;
                cen_block = sum_data[i + haar_ht][j + haar_wd];

                if (cen_block < sum_data[i][j])
                    lab_feat |= 1;
                if (cen_block < sum_data[i][j + haar_wd])
                    lab_feat |= 2;
                if (cen_block < sum_data[i][j + haar_wd * 2])
                    lab_feat |= 4;
                if (cen_block < sum_data[(i + haar_ht)][j + haar_wd * 2])
                    lab_feat |= 8;
                if (cen_block < sum_data[(i + 2 * haar_ht)][j + haar_wd * 2])
                    lab_feat |= 16;
                if (cen_block < sum_data[(i + 2 * haar_ht)][j + haar_wd])
                    lab_feat |= 32;
                if (cen_block < sum_data[(i + 2 * haar_ht)][j])
                    lab_feat |= 64;
                if (cen_block < sum_data[(i + haar_ht)][j])
                    lab_feat |= 128;
                lab_data[i][j] = lab_feat;
            }
        for (i = 0; i < ht + 1 - 3 * haar_ht; ++i)
            for (j = 0; j < wd + 1 - 3 * haar_wd; ++j) {
                lab_feat = 0;
                cen_block = sum_data_haar_x[(i + haar_ht)][j + haar_wd];

                if (cen_block < sum_data_haar_x[i][j])
                    lab_feat |= 1;
                if (cen_block < sum_data_haar_x[i][j + haar_wd])
                    lab_feat |= 2;
                if (cen_block < sum_data_haar_x[i][j + haar_wd * 2])
                    lab_feat |= 4;
                if (cen_block < sum_data_haar_x[(i + haar_ht)][j + haar_wd * 2])
                    lab_feat |= 8;
                if (cen_block < sum_data_haar_x[(i + 2 * haar_ht)][j + haar_wd * 2])
                    lab_feat |= 16;
                if (cen_block < sum_data_haar_x[(i + 2 * haar_ht)][j + haar_wd])
                    lab_feat |= 32;
                if (cen_block < sum_data_haar_x[(i + 2 * haar_ht)][j])
                    lab_feat |= 64;
                if (cen_block < sum_data_haar_x[(i + haar_ht)][j])
                    lab_feat |= 128;
                lab_data_x[i][j] = lab_feat;
            }
        for (i = 0; i < ht + 1 - 3 * haar_ht; ++i)
            for (j = 0; j < wd + 1 - 3 * haar_wd; ++j) {
                lab_feat = 0;
                cen_block = sum_data_haar_y[(i + haar_ht)][j + haar_wd];

                if (cen_block < sum_data_haar_y[i][j])
                    lab_feat |= 1;
                if (cen_block < sum_data_haar_y[i][j + haar_wd])
                    lab_feat |= 2;
                if (cen_block < sum_data_haar_y[i][j + haar_wd * 2])
                    lab_feat |= 4;
                if (cen_block < sum_data_haar_y[(i + haar_ht)][j + haar_wd * 2])
                    lab_feat |= 8;
                if (cen_block < sum_data_haar_y[(i + 2 * haar_ht)][j + haar_wd * 2])
                    lab_feat |= 16;
                if (cen_block < sum_data_haar_y[(i + 2 * haar_ht)][j + haar_wd])
                    lab_feat |= 32;
                if (cen_block < sum_data_haar_y[(i + 2 * haar_ht)][j])
                    lab_feat |= 64;
                if (cen_block < sum_data_haar_y[(i + haar_ht)][j])
                    lab_feat |= 128;
                lab_data_y[i][j] = lab_feat;
            }
    }
}

void FreeArray_int(int **array, int row, int col) {
    int i;

    for (i = 0; i < row; ++i)
        free(array[i]);
    free(array);
}

int GetRectSumImage_Fast(int **sum_image, int top, int left, int bottom, int right) {
    int res = 0;

    res = sum_image[bottom][right];
    res -= sum_image[top][right];
    res -= sum_image[bottom][left];
    res += sum_image[top][left];

    return res;
}

void GetSumImages(int **image, int **sum_image, int wd, int ht) {
    int i, j;
    int temp_data;
    int *pColSum;

    pColSum = (int *) malloc(sizeof(int) * wd);
    memset(pColSum, 0, sizeof(int) * wd);
    for (i = 0; i < ht; ++i) {
        pColSum[0] += image[i][0];
        temp_data = pColSum[0];
        sum_image[i][0] = temp_data;
        for (j = 1; j < wd; ++j) {
            pColSum[j] += image[i][j];
            temp_data += pColSum[j];
            sum_image[i][j] = temp_data;
        }
    }
    free(pColSum);
}

void GetSumImages(unsigned char **image, int **sum_image, int wd, int ht) {
    int i, j;
    int temp_data;
    int *pColSum;

    pColSum = (int *) malloc(sizeof(int) * wd);
    memset(pColSum, 0, sizeof(int) * wd);
    for (i = 0; i < ht; ++i) {
        pColSum[0] += image[i][0];
        temp_data = pColSum[0];
        sum_image[i][0] = temp_data;
        for (j = 1; j < wd; ++j) {
            pColSum[j] += image[i][j];
            temp_data += pColSum[j];
            sum_image[i][j] = temp_data;
        }
    }
    free(pColSum);
}

unsigned char **f2b(int nr, int nc) {
    unsigned char **x;
    int i;

    x = (unsigned char **) calloc(nr, sizeof(unsigned char *));
    if (x == NULL)
        return NULL;

    for (i = 0; i < nr; i++) {
        x[i] = (unsigned char *) calloc(nc, sizeof(unsigned char));
        if (x[i] == NULL)
            return NULL;
    }

    return x;
}

int **f2i(int nr, int nc) {
    int **x;
    int i;

    x = (int **) calloc(nr, sizeof(int *));
    if (x == NULL)
        return NULL;

    for (i = 0; i < nr; i++) {
        x[i] = (int *) calloc(nc, sizeof(int));
        if (x[i] == NULL)
            return NULL;
    }
    return x;
}

void FreeArray_BYTE(unsigned char **array, int row, int col) {
    int i;

    for (i = 0; i < row; ++i)
        free(array[i]);
    free(array);
}

void PyramidDownSample_2(unsigned char **src, int src_wd, int src_ht, unsigned char **dst,
                         int dst_wd, int dst_ht,
                         int down_rate, int down_move) {
    int img_down_move = down_move * 2;
    int i, j, k, l, x, y;
    int value = 0;

    for (i = 0; i < dst_ht; ++i)
        for (j = 0; j < dst_wd; ++j) {
            value = 0;
            for (k = 0; k < down_rate; ++k)
                for (l = 0; l < down_rate; ++l) {
                    x = (j << down_move) + l;
                    y = (i << down_move) + k;
                    value += src[y][x];
                }
            dst[i][j] = (value >> img_down_move);
        }
}

//#define MAX_TIMES 8
#define SHIFTBITS    8
#define ROUND0(x)  (x>>SHIFTBITS)
//#define ROUND1(x)  (ROUND0(x))+1
#ifndef CLIP
#define CLIP(x) ( x<0 ? 0 : (x>255 ? 255 : x) )
#endif

void AlgFace_BilinearResize_8u_1D_1R(unsigned char **pSrcImg2, unsigned char **pDesImg2,
                                     int srcWidth, int srcHeight, int desWidth, int desHeight) {
    int i, j;
    int as, bs, a, b, n;
    unsigned char *lpDstBits;
    int nRateW, nRateH;
//	int nLine0, nLine1;
    int WeightTL, WeightTR, WeightBL, WeightBR;
    unsigned char *TopLeft, *TopRight, *BottomLeft, *BottomRight;
    unsigned char *pLine0;
    unsigned char *pLine1;
    unsigned char *pSrcImg, *pDesImg;

    int vldW;
    int vldH;

    pSrcImg = (unsigned char *) malloc(sizeof(char) * srcWidth * srcHeight);
    pDesImg = (unsigned char *) malloc(sizeof(char) * desWidth * desHeight);

    for (i = 0; i < srcHeight; ++i)
        memcpy(pSrcImg + i * srcWidth, pSrcImg2[i], sizeof(char) * srcWidth);
    lpDstBits = pDesImg;

    nRateW = (srcWidth << SHIFTBITS) / max(desWidth, 1);
    nRateH = (srcHeight << SHIFTBITS) / max(desHeight, 1);
    for (i = 0; i < desHeight; i++) {
        as = i * nRateH;
        a = as & ((1 << SHIFTBITS) - 1);

        vldH = ROUND0(as);
        if (vldH > srcHeight - 2) {
            vldH = srcHeight - 2;
        }

        pLine0 = pSrcImg + vldH * srcWidth; // nLine0 = ROUND0(as)*srcWidth;
        pLine1 = pLine0 + srcWidth; // ROUND1(as)*srcWidth;

        for (j = 0; j < desWidth; j++) {
            bs = j * nRateW;
            b = bs & ((1 << SHIFTBITS) - 1);

            WeightTL = (((1 << SHIFTBITS) - a) * ((1 << SHIFTBITS) - b)) >> SHIFTBITS;
            WeightTR = (((1 << SHIFTBITS) - a) * b) >> SHIFTBITS;
            WeightBL = (a * ((1 << SHIFTBITS) - b)) >> SHIFTBITS;
            WeightBR = (a * b) >> SHIFTBITS;

            vldW = ROUND0(bs);
            if (vldW > srcWidth - 2) {
                vldW = srcWidth - 2;
            }

            TopLeft = pLine0 + vldW; // pSrcImg + nLine0 + ROUND0(bs);
            TopRight = TopLeft + 1; // pSrcImg + nLine0 + ROUND1(bs);
            BottomLeft = pLine1 + vldW; // pSrcImg + nLine1 + ROUND0(bs);
            BottomRight = BottomLeft + 1; // pSrcImg + nLine1 + ROUND1(bs);
            n = (*TopLeft) * WeightTL + (*TopRight) * WeightTR + (*BottomLeft) * WeightBL +
                (*BottomRight) * WeightBR;

            (*lpDstBits) = (unsigned char) (CLIP(n >> SHIFTBITS));
            ++lpDstBits;
        }
    }
    for (i = 0; i < desHeight; ++i)
        memcpy(pDesImg2[i], pDesImg + i * desWidth, sizeof(char) * desWidth);

    free(pSrcImg);
    free(pDesImg);
    return;
}

void BilinearResizeImage_Down2(unsigned char **pSrc, unsigned char **pDes, int oriwd, int oriht,
                               int deswd, int desht,
                               int down_rate, int down_move) {
    unsigned char **down_image;
    int down_ht, down_wd;

    down_ht = oriht / down_rate;
    down_wd = oriwd / down_rate;

    down_image = f2b(down_ht, down_wd);

    PyramidDownSample_2(pSrc, oriwd, oriht,
                        down_image, down_wd, down_ht, down_rate, down_move);

    AlgFace_BilinearResize_8u_1D_1R(down_image, pDes, down_wd, down_ht, deswd, desht);

    FreeArray_BYTE(down_image, down_ht, down_wd);
}

SDPOINT GetRectMidPoint(DECTRECT rect) {
    SDPOINT pt;
    pt.x = (rect.left + rect.right) / 2;
    pt.y = (rect.top + rect.bottom) / 2;

    return pt;
}

SDPOINT GetSDPoint(int x, int y) {
    SDPOINT pt;
    pt.x = x;
    pt.y = y;

    return pt;
}

float getDot2LineDist(SDPOINT pt, float line[4]) {
    //?????? ax+by+c=0
    float a, b, c = 0;
    a = line[1];
    b = line[0] * (-1);
    c = line[0] * line[3] - line[2] * line[1];

    float px = pt.x;
    float py = pt.y;

    float dist = 0;

    dist = fabs(a * px + b * py + c) / sqrt(a * a + b * b);

    return dist;
}

void sortRect(DECTRECT *rect, SDPOINT *point, int n) /*???????????????????????????????*/
{

    int i, j;
    SDPOINT temp;
    DECTRECT tempRec;
    for (i = 0; i < n - 1; i++)

        for (j = i + 1; j < n; j++) /*??????????????*/

            if (point[i].y > point[j].y) {

                temp = point[i];
                tempRec = rect[i];
                point[i] = point[j];
                rect[i] = rect[j];
                point[j] = temp;
                rect[j] = tempRec;
            }
}

void sortRectXY(DECTRECT *rect, int n, int m) {
    int i, j, k;
    SDPOINT temp;
    DECTRECT tempRec;
    for (i = 0; i < n - 1; i++)

        for (j = i + 1; j < n; j++) /*??????????????*/

            if (rect[i].top > rect[j].top) {


                tempRec = rect[i];

                rect[i] = rect[j];

                rect[j] = tempRec;
            }

    for (i = 0; i < n / m; i++) {
        for (j = i * n / m; j < i * n / m + m - 1; j++) {
            for (k = j + 1; k < i * n / m + m; k++) /*??????????????*/

                if (rect[j].left > rect[k].left) {


                    tempRec = rect[j];

                    rect[j] = rect[k];

                    rect[k] = tempRec;
                }
        }
    }
}

bool JudgeDotInRect(DECTRECT rect, SDPOINT point) {
    if (point.x > rect.left && point.x < rect.right && point.y > rect.top && point.y < rect.bottom)
        return true;
    else
        return false;
}

DECTRECT GetSubRect(DECTRECT inRect) {
    DECTRECT outRect;
    int width = inRect.right - inRect.left;
    int height = inRect.bottom - inRect.top;
    outRect.left = inRect.left + width / 4;
    outRect.right = inRect.right - width / 4;

    outRect.top = inRect.top + height / 4;
    outRect.bottom = inRect.bottom - height / 4;

    return outRect;
}